
// System headers
#include "stm32f10x.h"
#include "stm32f10x_adf.h"

// My headers
#include "main.h"
#include "led.h"
#include "button.h"
#include "adf.h"
#include "kl_lib.h"

int main(void) {
    GeneralInit();
//    LEDG_ON();

    while (1) {
        Adf.Task();             // Look for LOCKDETECT
        CmdUnit.Task();         // Uart cmd
        Btn.Task();             // Button enable
    }
}

void GeneralInit(void) {
    // Setup system clock
    RCC_HCLKConfig(RCC_SYSCLK_Div1);
    SystemCoreClockUpdate();
    klJtagDisable();

    Delay.Init();
    Delay.ms(63);

    Adf1424Mhz = &Adf.ADF1_PINs;           // Memory organizations for ADF PINs
    Adf248MHz = &Adf.ADF2_PINs;

    Adf.PortInit();                        // Unit ports to outputs ADF
    Adf.HardwareInit(Adf1424Mhz);
    Delay.us(42);
    Adf.HardwareInit(Adf248MHz);
    Delay.us(42);

    CmdUnit.Init();

    Led.Init();
    Btn.Init();

    Delay.ms(451);

    PLitBuf = LitBuf;

    LitBuf[0] = (0x80 | (Adf248MHz->Lithera*(-1)));   // FIXME
    LitBuf[1] = (0x80 | (Adf1424Mhz->Lithera*(-1)));

    AdfChip.Setup(Adf1424Mhz->Lithera, Adf1424Mhz);
    AdfChip.Setup(Adf248MHz->Lithera, Adf248MHz);

    CmdUnit.Printf("\rGLONASS Reciever v.%u\r", CURRENT_FIRMWARE_VERSION);
}

// ========================== Commands handling ================================
inline bool CharIsDecNumber(char c) {
    return ((c >= '0') and (c <= '9'));
}
inline bool CharIsHexNumber(char c) {
    return (((c >= '0') and (c <= '9')) or ((c >= 'A') and (c <= 'F')));
}
inline bool CharIsMinus(char c) {
    return (c == '-');
}
bool TryCommaStrToInt(char **Src, int *AResult) {
    int Fres = 0;
    char *S = *Src, c;
    for (uint8_t i=0; i<10; i++) {
        c = *S++;
        if ((c == 0) or (c == ',')) break;  // End of string
        if(!CharIsDecNumber(c)) return false;
        Fres = Fres*10 + (c - '0');
    }
    *AResult = Fres;
    *Src = S;
    return true;
}
bool TryCommaStrToUint(char **Src, uint32_t *AResult) {
    uint32_t Fres = 0;
    char *S = *Src, c;
    for (uint8_t i=0; i<10; i++) {
        c = *S++;
        if ((c == 0) or (c == ',')) break;  // End of string
        if(!CharIsDecNumber(c)) return false;
        Fres = Fres*10 + (c - '0');
    }
    *AResult = Fres;
    *Src = S;
    return true;
}
bool TryHexToByte(char **Src, uint8_t *AByte) {
    char *S = *Src, c;
    uint8_t b;
    c = *S++;
    if(!CharIsHexNumber(c)) return false;
    b = ((c >= '0') and (c <= '9'))? (c - '0') : ((c - 'A') + 10);
    b <<= 4;
    c = *S++;
    if(!CharIsHexNumber(c)) return false;
    b += ((c >= '0') and (c <= '9'))? (c - '0') : ((c - 'A') + 10);
    *AByte = b;
    *Src = S;
    return true;
}


void CmdUnit_t::NewCmdHandler() {
//    Printf("%S\r\n", CmdRcvd);    // Echo
    // If
    if((CmdRcvd[0] != 0xFE) && (CmdRcvd[1] != 0xFE)) { Printf("Wrong start pocket\r"); }

    else if((CmdRcvd[0] == 0xFE) && (CmdRcvd[1] == 0xFE)) {
        if (CmdRcvd[2] == 0x03) {
            PLitBuf[0] = (uint8_t)CmdRcvd[3];
            PLitBuf[1] = (uint8_t)CmdRcvd[4];
            // Turn into a int Lithera
            if(CmdRcvd[3] & 0x80) { // Lithera to 248 is below zero
                CmdRcvd[3] -= 0x80;
                Adf248MHz->Lithera = -(int)CmdRcvd[3];
            } else Adf248MHz->Lithera = (uint8_t)CmdRcvd[3];
            if(CmdRcvd[4] &  0x80) { // Lithera to 1424 is
                CmdRcvd[4] -= 0x80;
                Adf1424Mhz->Lithera = -(int)CmdRcvd[4];
            } else Adf1424Mhz->Lithera = (uint8_t)CmdRcvd[4];

            AdfChip.Setup(Adf248MHz->Lithera, Adf248MHz);
            AdfChip.Setup(Adf1424Mhz->Lithera, Adf1424Mhz);
            ATR(PLitBuf, CmdRcvd[2]);
        }
        else if(CmdRcvd[2] != 0x00) { // if correct ID
                ATR(PLitBuf, CmdRcvd[2]);
            }
    }
//    } else Printf(" ");
}


//    if (CmdRcvd[0] != '$') { Printf("$ERROR0\r\n"); return; }
//    // Kick
//    if (CmdRcvd[1] == 'K') Printf("$Ok\r\n");
//    // Ask
//    else if (CmdRcvd[1] == 'A') {
//        Printf("Litheras: \r\n %uMHz: %i\r\n %uMHz: %i\r\n", Adf1424Mhz->Freq, Adf1424Mhz->Lithera, Adf248MHz->Freq, Adf248MHz->Lithera);
//        Printf("$Ok\r\n");
//    }
//    // Set
//    else if (CmdRcvd[1] == 'S') {
//        if (CmdRcvd[2] != ',') { Printf("$ERROR1\r\n"); return; }
//        char *S = &CmdRcvd[3];
//        uint32_t FFreq=0;
//        int FLithera=0;
//        if (!CharIsDecNumber(*S))               { Printf("$ERROR2\r\n"); return; }
//        if (!TryCommaStrToUint(&S, &FFreq))     { Printf("$ERROR3\r\n"); return; }
//        if (CharIsMinus(*S)) {
//            S++;
//            if (!CharIsDecNumber(*S))               { Printf("$ERROR4.1\r\n"); return; }
//            if (!TryCommaStrToInt(&S, &FLithera))   { Printf("$ERROR5.1\r\n"); return; }
//            FLithera = 0 - FLithera;
//        }
//        else {
//            if (!CharIsDecNumber(*S))               { Printf("$ERROR4.2\r\n"); return; }
//            if (!TryCommaStrToInt(&S, &FLithera))   { Printf("$ERROR5.2\r\n"); return; }
//        }
//        Printf("\r\nSet: %u, %i\r\n", FFreq, FLithera);
//        if (FLithera > LITHERA_MAX) {Printf("$ERROR6.1\r\n"); return; }
//        if (FLithera < LITHERA_MIN) {Printf("$ERROR6.2\r\n"); return; }
//        if (FFreq == Adf1424Mhz->Freq) {
//            AdfChip.Setup(FLithera, Adf1424Mhz); Adf1424Mhz->Lithera = FLithera;
//        }
//        else if (FFreq == Adf248MHz->Freq) {
//            AdfChip.Setup(FLithera, Adf248MHz); Adf248MHz->Lithera = FLithera;
//        }
//        else {Printf("$ERROR7\r"); return; }
//        // Parsing ok, change tixe
//        Printf("$Ok\r\n");
//    }
//    else Printf("$ERROR: Unknown\r\n");
//}
